import sensor, image, time
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
import sensor, image, time, lcd, gc, cmath
import _thread
from fpioa_manager import fm
from machine import UART
from modules import ybkey
fm.register(6, fm.fpioa.UART2_RX)
fm.register(8, fm.fpioa.UART2_TX)
yb_uart = UART(UART.UART2, 9600, 8, 0, 0, timeout=2000, read_buf_len=2048)
write_bytes11 = [0xFF, 0x05, 0x00, 0x00, 0xFF, 0x00, 0x99, 0xE4]
write_bytes12 = [0xFF, 0x05, 0x00, 0x00, 0x00, 0x00, 0xD8, 0x14]
write_bytes31 = [0xFF, 0x05, 0x00, 0x01, 0xFF, 0x00, 0xC8, 0x24]
write_bytes32 = [0xFF, 0x05, 0x00, 0x01, 0x00, 0x00, 0x89, 0xD4]
write_bytes21 = [0xFF, 0x02, 0x01, 0x00, 0x90, 0x60]
write_bytes22 = [0xFF, 0x02, 0x01, 0x01, 0x51, 0xA0]
write_bytes41 = [0xFF, 0x02, 0x01, 0x00, 0x90, 0x60]
write_bytes42 = [0xFF, 0x02, 0x01, 0x00, 0x90, 0x60]
write_bytes4 = [0xFF, 0x02, 0x00, 0x00, 0x00, 0x08, 0x6C, 0x12]
write_bytes5 = [0xFF, 0x02, 0x00, 0x01, 0x00, 0x08, 0x3D, 0xD2]
last_time = time.ticks_ms()
gc.collect()
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((320, 240))
sensor.run(1)
sensor.skip_frames(10)
KEY=ybkey()
template_flag = 0
template_flag_arc = 0
try:
    template = image.Image("/sd/tt.jpg")
    template_gray = template.to_grayscale(0)
    template2 = image.Image("/sd/tt2.jpg")
    template_gray2 = template2.to_grayscale(0)
except:
    img=sensor.snapshot()
    img_roi=img.copy((120,70,90,90))
    img_roi.save("/sd/tt.jpg")
    img_roi.save("/sd/tt2.jpg")
    template = image.Image("/sd/tt.jpg")
    template_gray = template.to_grayscale(0)
    template2 = image.Image("/sd/tt2.jpg")
    template_gray2 = template2.to_grayscale(0)
clock = time.clock()
while(True):
    img=sensor.snapshot()
    img_1=img.copy()
    img_1.draw_rectangle((120,70,90,90),(0,0,255),2)
    if time.ticks_ms() - last_time >= 10:
        last_time = time.ticks_ms()
        read_data = None
        yb_uart.write(bytes(write_bytes4))
        time.sleep_ms(10)
        #if True:
        if yb_uart.any():#yb_uart.any()
           read_data = yb_uart.read()
           #if True:#read_data
           if read_data:
                #if True:#bytes(write_bytes22) == read_data
                print("bytes:",bytes(write_bytes22))
                print("read_data:",read_data)
                if bytes(write_bytes22) in  read_data:
                    img_gray = img.to_grayscale(0)
                    if template_flag_arc ==0:
                        data = img_gray.find_template(template_gray, 0.7, (110,60,110,110), 2, image.SEARCH_EX )
                        img_1.draw_string(20, 0,"L",color=(0,255,0),scale=4,mono_space=False)
                        if data != None:
                            img_1.draw_rectangle(data,(0,255,0),2)
                            img_1.draw_string(110, 0,"right",color=(0,255,0),scale=4,mono_space=False)
                            yb_uart.write(bytes(write_bytes11))
                            lcd.display(img_1)
                            time.sleep_ms(1000)
                            yb_uart.write(bytes(write_bytes12))
                            print(data)
                            template_flag_arc=1
                            test_time = time.ticks_ms()
                            print("test_time:",test_time)
                        else:
                            img_1.draw_string(100, 0,"wrong",color=(255,0,0),scale=4,mono_space=False)
                            img_1.draw_rectangle((120,70,90,90),(255,0,0),2)
                    else:
                        data = img_gray.find_template(template_gray2, 0.7, (110,60,110,110), 2, image.SEARCH_EX )
                        timeT=time.ticks_ms() - test_time
                        img_1.draw_string(20, 0,"R",color=(0,255,0),scale=4,mono_space=False)
                        print("test_time:",test_time)
                        print("time.ticks_ms():",time.ticks_ms())
                        if time.ticks_ms() - test_time >= 60000:
                                 print("change")
                                 template_flag_arc=0
                        if data != None:
                            img_1.draw_rectangle(data,(0,255,0),2)
                            img_1.draw_string(110, 0,"right",color=(0,255,0),scale=4,mono_space=False)
                            yb_uart.write(bytes(write_bytes11))
                            lcd.display(img_1)
                            time.sleep_ms(1000)
                            yb_uart.write(bytes(write_bytes12))
                            print(data)
                            template_flag_arc=0
                        else:
                            img_1.draw_string(100, 0,"wrong",color=(255,0,0),scale=4,mono_space=False)
                            img_1.draw_rectangle((120,70,90,90),(255,0,0),2)
    state = KEY.is_press()
    if state == 1:
        time.sleep_ms(5)
        state = KEY.is_press()
        if state == 1:
            img_roi = img.copy((120, 70, 90, 90))
            if template_flag == 0:
                img_roi.save("/sd/tt.jpg")
                template1 = image.Image("/sd/tt.jpg")
                template1_gray = template1.to_grayscale(0)
                template_flag = 1
                print("tt.jpg updated")
                img_1.draw_string(60, 200,"11111111",color=(0,255,0),scale=3,mono_space=False)
            else:
                img_roi.save("/sd/tt2.jpg")
                template2 = image.Image("/sd/tt2.jpg")
                template2_gray = template2.to_grayscale(0)
                template_flag = 0
                print("tt2.jpg updated")
                img_1.draw_string(60, 200,"22222222",color=(0,255,0),scale=3,mono_space=False)
            lcd.display(img_1)
    lcd.display(img_1)
clock = time.clock()
while(True):
    clock.tick()
    img = sensor.snapshot()
    print(clock.fps())
